#!/usr/bin/R

#set params/initial conditions
n.ponds=3 #number of ponds
area=c(100,100,100) #surface areas (m^2)
bottom.elev=c(0,0,0) #bottom elevations above datum (m)
weir.elev=c(1,1,1) #elevation of weirs above datum (m)
weir.width=c(1,1,1) #width of weirs (m)
sea.elev=1 #elevation on sea-side of final weir
t.final=24 #simulation length (hrs)
dt=0.1 #timestep length(hrs)
n.timesteps=as.integer(t.final/dt) #number of timesteps
h=array(0,dim=c(n.ponds+1,n.timesteps)) #initialize pond levels (m)
q=inflow=array(0,dim=c(n.ponds,n.timesteps)) #initialize outflows/inflows (cms)
#note: h[1:3,t]=pond elevations; h[4,t]=sea surface elevation 
h[1:3,1]=c(3,2,1) #initial pond elevations above datum (m)
inflow[1,]=1 #inflow to first pond for all times (cms)
h[4,]=1 #set sea-surface elev above datum for all times (m)
submerged=array(FALSE,dim=n.ponds)
#generate color array
library(fields)
my.colors=array(tim.colors(),dim=n.timesteps)

#plot initial conditions
plot(type='l',h[,1],col=my.colors[1])

#loop through time
for (t in 1:n.timesteps) {
	submerged=FALSE
	####for (n in n.ponds) {
		#determine whether discharge weir is submerged
			####if (h[j,t]>weir.elev[j] & h[j+1,t]>weir.elev[j]) submerged=TRUE
		submerged(which(h[1:(n.ponds-1),t]>weir.elev[1:(n.ponds-1)]) & which(h[2:(n.ponds-1),t]>weir.elev[1:(n.ponds-1)])) = TRUE
		#determine outflow
		####if (submerged) {#'francis' submerged weir formula
			i=which(submerged==TRUE)
			q[i,t] = 3.33*weir.width[i]*sqrt(h[i,t]-h[i+1,t])*((h[i,t]-weir.elev[i])+(h[i+1,t]-weir.elev[i])*.381)
		####}
		####if (!submerged) {#francis sharp-crested weir formula (assumes ~zero velocity)
			j=which(submerged==FALSE)
			q[j,t]=3.33*weir.width[j]*((h[j,t]-weir.elev[j])^(3/2))
		####}
		#determine new pond level	
			K1=dt/area[j]*(inflow[j,t]-q[j,t])
			if (submerged) 	q.temp=3.33*weir.width[j]*sqrt(h[j,t]+(1/3)*K1-h[j+1,t])*
					((h[j,t]+(1/3)*K1-weir.elev[j])+(h[j+1,t]-weir.elev[j])*.381)
			if (!submerged) q.temp=3.33*weir.width[j]*((h[j,t]+(1/3)*K1-weir.elev[j])^(3/2))
			K2=dt/area[j]*(inflow[j,t]-q.temp)
			if (submerged)  q.temp=3.33*weir.width[j]*sqrt(h[j,t]+(2/3)*K1-h[j+1,t])*
                                        ((h[j,t]+(2/3)*K1-weir.elev[j])+(h[j+1,t]-weir.elev[j])*.381)
			if (!submerged) q.temp=3.33*weir.width[j]*((h[j,t]+(2/3)*K2-weir.elev[j])^(3/2))
			K3=dt/area[j]*(inflow[j,t]-q.temp)
			h(j,t+1)=h(j,t)+(1/4)*K1+(3/4)*K3
		#determine outflow @ end of timestep
			if (submerged)  q.temp=3.33*weir.width[j]*sqrt(h[j,t+1]-h[j+1,t])*
                                        ((h[j,t+1]-weir.elev[j])+(h[j+1,t]-weir.elev[j])*.381)
			q.temp=3.33*weir.width[j]*((h[j,t+1]-weir.elev[j])^(3/2))	
		#if 'new' outflow is very different....???
			#DO SOMETHING HERE#
		#between ponds, set inflow equal to outflow of upstream pond avg'd over timestep
			inflow[j+1,t]=(q[j,t]+q.temp)/2
	}
lines(h[,t],col=my.colors[t+1])
}
